#!/usr/bin/env python


import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0
 
X_t = 0
Y_t = 0
X_t_Pre = 0
Y_t_Pre = 0
X_sim = [-1.1, -0.9, -0.6, -0.2, 0.1, 0.4, 0.6, 0.7, 0.8, 0.9 , 1.1, 1.3, 1.5]
Y_sim = [1.2 , 1.1 , 0.9 , 0.7 , 0.6, 0.3, 0.0, -0.1,-0.4,-0.6,-0.9,-1.2,-1.0]
r = 0
 
 
def Trans_robot_pose(msg):
    global x
    global y
    global w_o
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y
 
 
if __name__ == '__main__':
    rospy.init_node('item1')
 
    turtle_vel = rospy.Publisher('/tb3_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)
 
    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()
        (roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])
        if yaw < 0:
            yaw = yaw + 2 * math.pi
 
        X_t = X_sim[r]
        Y_t = Y_sim[r]
 
        D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))
 
        if (Y_t - y) == 0 and (X_t - x) > 0:
            yaw_t = 0
        if (Y_t - y) > 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x))
        if (Y_t - y) > 0 and (X_t - x) == 0:
            yaw_t = 0.5 * math.pi
        if (Y_t - y) > 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) == 0 and (X_t - x) < 0:
            yaw_t = math.pi
        if (Y_t - y) < 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) < 0 and (X_t - x) == 0:
            yaw_t = 1.5 * math.pi
        if (Y_t - y) < 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi
 
        Theta_err = yaw_t - yaw
 
        if Theta_err < -math.pi:
            Theta_err = Theta_err + 2 * math.pi
        if Theta_err > math.pi:
            Theta_err = Theta_err - 2 * math.pi
 
        if D_err < 0.3:    
            X_t_Pre = X_t
            Y_t_Pre = Y_t
            r = r + 1
            print r
            if r == 10:
                r = 0
 
        liner_speed = 0.1 * D_err
        angular_speed = 0.7 * Theta_err
 
        msg.linear.x = liner_speed
        msg.angular.z = angular_speed
 
        liner_speed_old = liner_speed
        angular_speed_old = angular_speed
        turtle_vel.publish(msg)
        rospy.Subscriber('/tb3_0/odom', nav_msgs.msg.Odometry,  Trans_robot_pose)
 
        rate.sleep()
    rospy.spin()
 
